#ifndef MAXIT2
#define MAXIT2 10000
#define _THRES2 0.00001
#endif
//#ifndef LaplaceLS
//#define LaplaceLS
#include <armadillo>

using namespace arma;
class LaplaceLS
{
	public:

	LaplaceLS()
	{	
		boost::math::normal_distribution<> *d=new boost::math::normal_distribution<>(0,1); 
		_d=d;
		_conv=0;
	}
	~LaplaceLS()
	{
		delete _d;
	}
	void operator()(mat X,double *Y,mat m,double si,mat init)
	{
		double e=1;
		double thres=_THRES2;
		int p=X.n_cols;
		mat theta(p,1);
		mat Ym=vect2mat<double>(Y,X.n_rows,1);
		if(init(0,0)==2.3302)
		{
			theta=inv(X.t()*X)*X.t()*Ym;
		}else{
			theta=init;
		}
		mat b(p,1);
		mat dk(p,1);
		int i=0;
		double lambda;
		mat ltheta=theta;
		mat sdp=Hessian(X,Y,theta,si);	
		mat keepg=Gradiant(X,Y,theta,si);
		while(e>thres & MAXIT2>i)
		{
					
			//sdp=-sdp.t()*sdp;
		mat grad=Gradiant(X,Y,theta,si);
			mat y=grad-keepg;
			mat s=theta-ltheta;
		if(i!=0)
		{	
				sdp=sdp-sdp*s*s.t()*sdp/(as_scalar(s.t()*sdp*s))+y*y.t()/as_scalar(y.t()*s);
		}


	//	mat sdp=inv(Hessian(X,Y,theta,si));	
			/*	int o=0;
			cx_vec eigval;
			cx_mat eigvec;
			eig_gen(eigval, eigvec, sdp);
			for(int j=0;j<p;j++)
			{
				double t=real(eigval(j));
				if(t<0)
				{
					eigval(j)*=-1;
					o=1;
				}
			}
			sdp=real(eigvec*diagmat(eigval)*eigvec.t());	
		//	if(o){sdp=sdp.eye();}
		*/
	
			dk=-inv(sdp)*grad;
			//	cout <<"dk " << dk;
			lambda=LineSearch(dk,theta,X,Y,si,thres);
			b=theta+lambda*dk;
			ltheta=theta;
			e=abserr(theta-b);
			
			theta=b;
			keepg=grad;
			i++;
			cout << "e " << e << " ";
		}
		if(i>MAXIT){cout << "reached max iteration!";_conv=1;	}
		_m=theta;
		_S=-inv(Hessian2(X,Y,theta,si));
		//_S=-inv(sdp);

	}
	double LineSearch(mat dk , mat theta ,mat X, double *Y, double si,double thres)
	{
		double u=0.000001;
		double res=-1;
		while(res<0)
		{
			u*=2;
		       	res=dot(-Gradiant(X,Y,theta+u*dk,si),dk);
			//cout << "resp "<< res;
		}
		double l=0;
		//cout << "res " << dot(-Gradiant(X,Y,theta+l*dk,si),dk);

		double th=0.0001;
		double lambda=0;
		double e=1;
		double res2;
		while(e>th)
		{
			lambda=(u+l)/2;
			res=dot(-Gradiant(X,Y,theta+lambda*dk,si),dk);
			//cout << res;
			if(res<0)
			{
				l=lambda;
			}else{
				u=lambda;
			}
			e=sqrt(pow(res-res2,2));
			res2=res;
		}
		double newl=(l+u)/2;
		//cout <<"newl "<< newl;
		 //cout << "res2 "<< dot(-Gradiant(X,Y,theta+newl*dk,si),dk);
		return newl;

	}
	


	inline double abserr(mat X)
	{
		double sum=0;
		int n=X.n_rows;
		for(int i=0;i<n;i++)
		{
			sum+=abs<double>(X(i,0));
		}
		return sum;
	}

	inline mat Gradiant(mat X,double *Y,mat theta,double si)
	{
		int p=X.n_cols;
		mat sum(p,1);
		sum.fill(0);
		int n=X.n_rows;
		double xb=0;
		for(int i=0;i<n;i++)
		{
			xb=as_scalar(X(i,span::all)*theta);
			double pxb=pdf(*_d,xb);
			double gpxb=cdf(*_d,xb);
				
			if(Y[i]==1)
			{	
				sum=sum+pxb*(1/gpxb)*X(i,span::all).t();				
			}else{
							
				sum=sum-pxb*(1/(1-gpxb))*X(i,span::all).t();				
			}
		}
		return sum-as_scalar(arma::sum(theta,0))/si;
		
	}

	inline	mat Hessian(mat X,double *Y,mat theta,double si)
	{
		int p=X.n_cols;
		mat sum(p,p);
		sum.fill(0);
		int n=X.n_rows;
		double xb=0;
		for(int i=0;i<n;i++)
		{
			
			xb=as_scalar(X(i,span::all)*theta);
			double pxb=pdf(*_d,xb);
			double gpxb=cdf(*_d,xb);
			mat xx=X(i,span::all).t()*X(i,span::all);	
			if(Y[i]==1)
			{	
				sum=sum+(xb*pxb/gpxb-pow(pxb,2)/pow(gpxb,2))*xx;				
			}else{
				sum=sum-(xb*pxb/(1-gpxb)+pow(pxb,2)/pow(1-gpxb,2))*xx;				
			}
		}
		return sum-(double)1/si;

	}
	inline	mat Hessian2(mat X,double *Y,mat theta,double si)
	{
		int p=X.n_cols;
		mat sum(p,p);
		sum.fill(0);
		int n=X.n_rows;
		double xb=0;
		for(int i=0;i<n;i++)
		{
			
			xb=as_scalar(X(i,span::all)*theta);
			double pxb=pdf(*_d,xb);
			double gpxb=cdf(*_d,xb);
			mat xx=X(i,span::all).t()*X(i,span::all);	
			if(Y[i]==1)
			{	
				sum=sum+(xb*pxb/gpxb+pow(pxb,2)/pow(gpxb,2))*xx;				
			}else{
				sum=sum+(-xb*pxb/(1-gpxb)+pow(pxb,2)/pow(1-gpxb,2))*xx;				
			}
		}
		return -sum-(double)1/si;

	}
	
	mat Get_Sig(void){return _S;}
	mat Get_Mu(void){return _m;}
	int Get_conv(void){return _conv;}
	private:
	mat _m;
	mat _S;
	int _conv;
	boost::math::normal_distribution<> *_d; 
};
//#endif
